// 文件名: sj_image.c
// 职责: 实现所有图像处理算法，并提供接口
// 版本: 1.0 (修改后)

// =================== 头文件区 ===================
#include "main.h"            // <--- 【修改1】必须包含它，以链接全局变量
#include "zf_common_typedef.h" 
#include "sj_image.h"        
#include "sj_camera_param.h" 
#include "sj_imgproc.h"      
#include <math.h>            
#include "sj_utils.h"


// ====================================================================
// =================== 1. 全局变量定义区 (唯一拥有者) ===================
// ====================================================================
// 这些变量在本文件中【定义】，在 main.h 中通过 extern【声明】

// 图像数据结构
image_t img_raw;
image_t img_line;

// 算法可调参数 (这些是本模块私有的，保持 static)
/* static */ float thres = 140.0f;
/* static */ float block_size = 3.0f;
/* static */ float clip_value = 2.0f;
/* static */ float begin_x = 15.0f;
/* static */ float begin_y = 80.0f;
/* static */ float line_blur_kernel = 3.0f;
/* static */ float pixel_per_meter = 102.0f;
/* static */ float sample_dist = 0.02f;
/* static */ float angle_dist = 0.2f;

#define ROAD_WIDTH (0.4f) // 这个也需要全局访问

// --- 【修改2】移除大部分变量前的 static 关键字，使其成为全局变量 ---

// 原始图像点集
AT_DTCM_SECTION_ALIGN(int ipts0[POINTS_MAX_LEN][2], 8);
AT_DTCM_SECTION_ALIGN(int ipts1[POINTS_MAX_LEN][2], 8);
int ipts0_num, ipts1_num;

// 变换后点集
AT_DTCM_SECTION_ALIGN(float rpts0[POINTS_MAX_LEN][2], 8);
AT_DTCM_SECTION_ALIGN(float rpts1[POINTS_MAX_LEN][2], 8);
int rpts0_num, rpts1_num;

// 滤波后点集
AT_DTCM_SECTION_ALIGN(float rpts0b[POINTS_MAX_LEN][2], 8);
AT_DTCM_SECTION_ALIGN(float rpts1b[POINTS_MAX_LEN][2], 8);
int rpts0b_num, rpts1b_num;

// 重采样后点集
AT_DTCM_SECTION_ALIGN(float rpts0s[POINTS_MAX_LEN][2], 8);
AT_DTCM_SECTION_ALIGN(float rpts1s[POINTS_MAX_LEN][2], 8);
int rpts0s_num, rpts1s_num;

// 左右中线
AT_DTCM_SECTION_ALIGN(float rptsc0[POINTS_MAX_LEN][2], 8);
AT_DTCM_SECTION_ALIGN(float rptsc1[POINTS_MAX_LEN][2], 8);
int rptsc0_num, rptsc1_num;

// 角度变化率 (这些是中间过程，可以保持 static)
AT_DTCM_SECTION_ALIGN(static float rpts0a[POINTS_MAX_LEN], 8);
AT_DTCM_SECTION_ALIGN(static float rpts1a[POINTS_MAX_LEN], 8);
static int rpts0a_num, rpts1a_num;

// 非极大值抑制后 (中间过程，保持 static)
AT_DTCM_SECTION_ALIGN(static float rpts0an[POINTS_MAX_LEN], 8);
AT_DTCM_SECTION_ALIGN(static float rpts1an[POINTS_MAX_LEN], 8);
static int rpts0an_num, rpts1an_num;

// 角点信息
int Ypt0_rpts0s_id, Ypt1_rpts1s_id;
bool Ypt0_found, Ypt1_found;
int Lpt0_rpts0s_id, Lpt1_rpts1s_id;
bool Lpt0_found, Lpt1_found;
bool is_straight0, is_straight1;

// --- 【修改3】清理掉了所有重复的变量定义 ---


// =================== 2. 模块内部的私有函数 ===================

/**
 * @brief 执行图像处理的核心算法流程 (移植自 process_image)
 */
static void process_image_internal(void) 
{
    // 原图找左右边线
    int x1 = img_raw.width / 2 - begin_x, y1 = begin_y;
    ipts0_num = sizeof(ipts0) / sizeof(ipts0[0]);
    for (; x1 > 0; x1--) if (AT_IMAGE(&img_raw, x1 - 1, y1) < thres) break;
    if (AT_IMAGE(&img_raw, x1, y1) >= thres)
        findline_lefthand_adaptive(&img_raw, block_size, clip_value, x1, y1, ipts0, &ipts0_num);
    else ipts0_num = 0;
    
    int x2 = img_raw.width / 2 + begin_x, y2 = begin_y;
    ipts1_num = sizeof(ipts1) / sizeof(ipts1[0]);
    for (; x2 < img_raw.width - 1; x2++) if (AT_IMAGE(&img_raw, x2 + 1, y2) < thres) break;
    if (AT_IMAGE(&img_raw, x2, y2) >= thres)
        findline_righthand_adaptive(&img_raw, block_size, clip_value, x2, y2, ipts1, &ipts1_num);
    else ipts1_num = 0;

    // 去畸变+透视变换
    for (int i = 0; i < ipts0_num; i++) {
        rpts0[i][0] = mapx[ipts0[i][1]][ipts0[i][0]];
        rpts0[i][1] = mapy[ipts0[i][1]][ipts0[i][0]];
    }
    rpts0_num = ipts0_num;
    for (int i = 0; i < ipts1_num; i++) {
        rpts1[i][0] = mapx[ipts1[i][1]][ipts1[i][0]];
        rpts1[i][1] = mapy[ipts1[i][1]][ipts1[i][0]];
    }
    rpts1_num = ipts1_num;

    // 边线滤波
    blur_points(rpts0, rpts0_num, rpts0b, (int) round(line_blur_kernel));
    rpts0b_num = rpts0_num;
    blur_points(rpts1, rpts1_num, rpts1b, (int) round(line_blur_kernel));
    rpts1b_num = rpts1_num;

    // 边线等距采样
    rpts0s_num = sizeof(rpts0s) / sizeof(rpts0s[0]);
    resample_points(rpts0b, rpts0b_num, rpts0s, &rpts0s_num, sample_dist * pixel_per_meter);
    rpts1s_num = sizeof(rpts1s) / sizeof(rpts1s[0]);
    resample_points(rpts1b, rpts1b_num, rpts1s, &rpts1s_num, sample_dist * pixel_per_meter);

    // 边线局部角度变化率
    local_angle_points(rpts0s, rpts0s_num, rpts0a, (int) round(angle_dist / sample_dist));
    rpts0a_num = rpts0s_num;
    local_angle_points(rpts1s, rpts1s_num, rpts1a, (int) round(angle_dist / sample_dist));
    rpts1a_num = rpts1s_num;

    // 角度变化率非极大抑制
    nms_angle(rpts0a, rpts0a_num, rpts0an, (int) round(angle_dist / sample_dist) * 2 + 1);
    rpts0an_num = rpts0a_num;
    nms_angle(rpts1a, rpts1a_num, rpts1an, (int) round(angle_dist / sample_dist) * 2 + 1);
    rpts1an_num = rpts1a_num;

    // 左右中线跟踪
    track_leftline(rpts0s, rpts0s_num, rptsc0, (int) round(angle_dist / sample_dist), pixel_per_meter * ROAD_WIDTH / 2);
    rptsc0_num = rpts0s_num;
    track_rightline(rpts1s, rpts1s_num, rptsc1, (int) round(angle_dist / sample_dist), pixel_per_meter * ROAD_WIDTH / 2);
    rptsc1_num = rpts1s_num;
}

/**
 * @brief 寻找角点和直道的核心算法 (移植自 find_corners)
 */
static void find_corners_internal() 
{
    Ypt0_found = Ypt1_found = Lpt0_found = Lpt1_found = false;
    is_straight0 = rpts0s_num > 1. / sample_dist;
    is_straight1 = rpts1s_num > 1. / sample_dist;

    for (int i = 0; i < rpts0s_num; i++) {
        if (rpts0an[i] == 0) continue;
        int im1 = clip(i - (int) round(angle_dist / sample_dist), 0, rpts0s_num - 1);
        int ip1 = clip(i + (int) round(angle_dist / sample_dist), 0, rpts0s_num - 1);
        float conf = fabs(rpts0a[i]) - (fabs(rpts0a[im1]) + fabs(rpts0a[ip1])) / 2;

        if (Ypt0_found == false && 30. / 180. * PI < conf && conf < 65. / 180. * PI && i < 0.8 / sample_dist) {
            Ypt0_rpts0s_id = i; Ypt0_found = true;
        }
        if (Lpt0_found == false && 70. / 180. * PI < conf && conf < 140. / 180. * PI && i < 0.8 / sample_dist) {
            Lpt0_rpts0s_id = i; Lpt0_found = true;
        }
        if (conf > 5. / 180. * PI && i < 1.0 / sample_dist) is_straight0 = false;
        if (Ypt0_found == true && Lpt0_found == true && is_straight0 == false) break;
    }

    for (int i = 0; i < rpts1s_num; i++) {
        if (rpts1an[i] == 0) continue;
        int im1 = clip(i - (int) round(angle_dist / sample_dist), 0, rpts1s_num - 1);
        int ip1 = clip(i + (int) round(angle_dist / sample_dist), 0, rpts1s_num - 1);
        float conf = fabs(rpts1a[i]) - (fabs(rpts1a[im1]) + fabs(rpts1a[ip1])) / 2;
        if (Ypt1_found == false && 30. / 180. * PI < conf && conf < 65. / 180. * PI && i < 0.8 / sample_dist) {
            Ypt1_rpts1s_id = i; Ypt1_found = true;
        }
        if (Lpt1_found == false && 70. / 180. * PI < conf && conf < 140. / 180. * PI && i < 0.8 / sample_dist) {
            Lpt1_rpts1s_id = i; Lpt1_found = true;
        }
        if (conf > 5. / 180. * PI && i < 1.0 / sample_dist) is_straight1 = false;
        if (Ypt1_found == true && Lpt1_found == true && is_straight1 == false) break;
    }

    // Y点二次检查
    if (Ypt0_found && Ypt1_found) {
        float dx = rpts0s[Ypt0_rpts0s_id][0] - rpts1s[Ypt1_rpts1s_id][0];
        float dy = rpts0s[Ypt0_rpts0s_id][1] - rpts1s[Ypt1_rpts1s_id][1];
        float dn = sqrtf(dx * dx + dy * dy);
        if (fabs(dn - 0.45 * pixel_per_meter) > 0.15 * pixel_per_meter) {
            Ypt0_found = Ypt1_found = false;
        }
    }

    // L点二次检查
    if (Lpt0_found && Lpt1_found) {
        float dx = rpts0s[Lpt0_rpts0s_id][0] - rpts1s[Lpt1_rpts1s_id][0];
        float dy = rpts0s[Lpt0_rpts0s_id][1] - rpts1s[Lpt1_rpts1s_id][1];
        float dn = sqrtf(dx * dx + dy * dy);
        if (fabs(dn - 0.45 * pixel_per_meter) > 0.15 * pixel_per_meter) {
            Lpt0_found = Lpt1_found = false;
        }
    }
}


// =================== 3. 对外接口的实现 ===================

void sf_img_logic_init(void)
{
    // 初始化 img_raw 结构体，除了数据指针
    img_raw = DEF_IMAGE(NULL, MT9V03X_W, MT9V03X_H);
}

void sf_img_logic_process(uint8_t* raw_image_data)
{
    // 每次调用时，更新图像数据指针
    img_raw.data = raw_image_data;

    // 依次执行内部的算法流程
    process_image_internal();
    find_corners_internal();
}

void sf_get_left_line_resampled(float** left_line_ptr, int* num)
{
    *left_line_ptr = &rpts0s[0][0];
    *num = rpts0s_num;
}

void sf_get_right_line_resampled(float** right_line_ptr, int* num)
{
    *right_line_ptr = &rpts1s[0][0];
    *num = rpts1s_num;
}

void sf_get_final_center_line(float** center_line_ptr, int* num)
{
    // 注意：逐飞的最终中线 rptsn 是在 main.c 的主循环里根据巡线模式计算的。
    // 为了解耦，我们在这里暂时不实现它，而是先返回一个基础中线，比如右中线。
    // 后续您的主程序 main.c 在获取了左右边线后，可以自己计算最终中线。
    // 这里我们先返回一个空值，表示需要主程序自己计算。
    *center_line_ptr = NULL;
    *num = 0;
}

bool sf_is_left_L_found(int* out_id)
{
    if (Lpt0_found && out_id != NULL) {
        *out_id = Lpt0_rpts0s_id;
    }
    return Lpt0_found;
}

bool sf_is_right_L_found(int* out_id)
{
    if (Lpt1_found && out_id != NULL) {
        *out_id = Lpt1_rpts1s_id;
    }
    return Lpt1_found;
}

bool sf_is_left_Y_found(int* out_id)
{
    if (Ypt0_found && out_id != NULL) {
        *out_id = Ypt0_rpts0s_id;
    }
    return Ypt0_found;
}

bool sf_is_right_Y_found(int* out_id)
{
    if (Ypt1_found && out_id != NULL) {
        *out_id = Ypt1_rpts1s_id;
    }
    return Ypt1_found;
}

bool sf_is_left_straight(void)
{
    return is_straight0;
}

bool sf_is_right_straight(void)
{
    return is_straight1;
}

bool sf_is_zebra_found(void)
{
    // 移植斑马线检测逻辑
    static int64_t last_zebra_encoder = 0;
    // (假设在主工程中可以调用 get_total_encoder())
    // if (get_total_encoder() - last_zebra_encoder < ENCODER_PER_METER * 0.5) {
    //     return false;
    // }
    
    const int y_scan_line = MT9V03X_H / 2;
    int transitions = 0;
    uint8_t last_pixel_is_white = (AT_IMAGE(&img_raw, 0, y_scan_line) > thres);

    for (int x = 1; x < MT9V03X_W; ++x) {
        uint8_t current_pixel_is_white = (AT_IMAGE(&img_raw, x, y_scan_line) > thres);
        if (current_pixel_is_white != last_pixel_is_white) {
            transitions++;
        }
        last_pixel_is_white = current_pixel_is_white;
    }

    if (transitions > 10) {
        // last_zebra_encoder = get_total_encoder();
        return true;
    }
    return false;
}
// =================== 调试绘图函数 (之前遗漏的实现) ===================

void sf_draw_debug_view(uint8_t* output_buffer)
{
    // 1. 将传入的缓冲区包装成 image_t 结构，方便使用 AT_IMAGE 宏
    image_t img_debug;
    img_debug.data = output_buffer;
    img_debug.width = 376;
    img_debug.height = 240;
    img_debug.step = 376;

    // 2. 清空图像 (变为全黑)
    // 假设 clear_image 函数在 sj_imgproc.c 中
    clear_image(&img_debug); 

    // 3. 绘制左边线 (rpts0s)
    for (int i = 0; i < rpts0s_num; i++) {
        AT_IMAGE(&img_debug, clip(rpts0s[i][0], 0, img_debug.width - 1),
                 clip(rpts0s[i][1], 0, img_debug.height - 1)) = 255;
    }

    // 4. 绘制右边线 (rpts1s)
    for (int i = 0; i < rpts1s_num; i++) {
        AT_IMAGE(&img_debug, clip(rpts1s[i][0], 0, img_debug.width - 1),
                 clip(rpts1s[i][1], 0, img_debug.height - 1)) = 255;
    }
    
    // =====================================================================
    // ==================== 在这里添加新的中线显示代码 ====================
    // =====================================================================
    // 注意：灰度图无法显示“红”和“绿”，但我们可以用不同的灰度值来区分它们。
    // 比如，浅灰色代表一种，深灰色代表另一种。
    // 我们用 200 代表亮灰色 (模拟绿色)，100 代表暗灰色 (模拟红色)。

    // 5. 绘制由右线计算出的中线 (rptsc1) - 亮灰色 (模拟绿色)
    for (int i = 0; i < rptsc1_num; i++) {
        AT_IMAGE(&img_debug, clip(rptsc1[i][0], 0, img_debug.width - 1),
                 clip(rptsc1[i][1], 0, img_debug.height - 1)) = 200; // 亮灰色
    }
    
    // 6. 绘制由左线计算出的中线 (rptsc0) - 暗灰色 (模拟红色)
    for (int i = 0; i < rptsc0_num; i++) {
        AT_IMAGE(&img_debug, clip(rptsc0[i][0], 0, img_debug.width - 1),
                 clip(rptsc0[i][1], 0, img_debug.height - 1)) = 100; // 暗灰色
    }

    // =====================================================================
    // ============================ 新代码结束 ============================
    // =====================================================================

    // 6. 绘制L角点 (用十字标记)
    if (Lpt0_found) {
        draw_x(&img_debug, rpts0s[Lpt0_rpts0s_id][0], rpts0s[Lpt0_rpts0s_id][1], 5, 255);
    }
    if (Lpt1_found) {
        draw_x(&img_debug, rpts1s[Lpt1_rpts1s_id][0], rpts1s[Lpt1_rpts1s_id][1], 5, 255);
    }
}
// ... (sf_draw_debug_view 函数的实现)

// --- 新增函数的实现 ---
void sf_get_raw_left_line(int** left_line_ptr, int* num)
{
    *left_line_ptr = &ipts0[0][0];
    *num = ipts0_num;
}

void sf_get_raw_right_line(int** right_line_ptr, int* num)
{
    *right_line_ptr = &ipts1[0][0];
    *num = ipts1_num;
}

void sf_get_left_center_line(float** left_center_line_ptr, int* num) {
    *left_center_line_ptr = &rptsc0[0][0];
    *num = rptsc0_num;
}

void sf_get_right_center_line(float** right_center_line_ptr, int* num) {
    *right_center_line_ptr = &rptsc1[0][0];
    *num = rptsc1_num;
}
